/*
      ____                      _____                  +---+
     / ___\                     / __ \                 | R |
    / /                        / /_/ /                 +---+
   / /   ________  ____  ___  / ____/___  ____  __   __
  / /  / ___/ __ `/_  / / _ \/ /   / __ \/ _  \/ /  / /
 / /__/ /  / /_/ / / /_/  __/ /   / /_/ / / / / /__/ /
 \___/_/   \__,_/ /___/\___/_/    \___ /_/ /_/____  /
                                                 / /
                                            ____/ /
                                           /_____/
main.c file
编写者：小马  (Camel)
作者E-mail：375836945@qq.com
编译环境：MDK-Lite  Version: 4.23
初版时间: 2014-01-28
功能：
------------------------------------
*/
#include "config.h"             //包含所有的驱动头文件
#include "Led.h"
#include "Pwm.h"
#include "DHT11.h"
#include "I2C_MPU6050.h"
#include "DataScope_DP.h"
#include "iic.h"
#include "mpu6050.h"
#include "filter.h"
#include "calculate.h"


#define GX_OFFSET 0x01
#define AX_OFFSET 0x01
#define AY_OFFSET 0x01
#define AZ_OFFSET 0x01


unsigned char i;      //计数变量
unsigned char Send_Count; //串口需要发送的数据个数
static float a;

u8 receive_data;
u8 flg_get_senor_data;
u8 out[35]  ={0x5f, 0x60, 0};
float angle, angle_dot, f_angle, f_angle_dot;
s16 temp; 
s16 gx, gy, gz, ax ,ay, az, temperature;

#define FILTER_COUNT  16
s16 gx_buf[FILTER_COUNT], ax_buf[FILTER_COUNT], ay_buf[FILTER_COUNT],az_buf[FILTER_COUNT];

void iic_init(void);
void acc_filter(void)
{
  u8 i;
  s32 ax_sum = 0, ay_sum = 0, az_sum = 0; 

  for(i = 1 ; i < FILTER_COUNT; i++)
  {
    ax_buf[i - 1] = ax_buf[i];
	ay_buf[i - 1] = ay_buf[i];
	az_buf[i - 1] = az_buf[i];
  }

  ax_buf[FILTER_COUNT - 1] = ax;
  ay_buf[FILTER_COUNT - 1] = ay;
  az_buf[FILTER_COUNT - 1] = az;

  for(i = 0 ; i < FILTER_COUNT; i++)
  {
    ax_sum += ax_buf[i];
	ay_sum += ay_buf[i];
	az_sum += az_buf[i];
  }

  ax = (s16)(ax_sum / FILTER_COUNT);
  ay = (s16)(ay_sum / FILTER_COUNT);
  az = (s16)(az_sum / FILTER_COUNT);
}
int main(void)
{
	int status = 0;
	/***********************************/
	SystemClock_HSI(9);           //系统时钟初始化，时钟源内部HSI
	
	cycleCounterInit();				    // Init cycle counter
	SysTick_Config(SystemCoreClock / 1000);	//SysTick开启系统tick定时器并初始化其中断，1ms
	UART1_init(SysClock,uart1baudSet); //串口1初始化
  NVIC_INIT();	                //中断初始化
	//KeyInit();										//按键初始化
	LedInit();
	
#ifdef UART_DEBUG  
	TIM3_Init(SysClock,300);			//定时器初始化，1s为周期打印摇杆值
#endif
	TIM4_Init(SysClock,TIME4_Preiod);	  //定时器4初始化，定时时间单位：(TIME4_Preiod)微秒
	I2C_MPU6050_Init();
	InitMPU6050();	
	while(1)
		{
			
		gx = GetData(GYRO_XOUT_H);
		gy = GetData(GYRO_YOUT_H);
		gz = GetData(GYRO_ZOUT_H);
		ax = GetData(ACCEL_XOUT_H);
		ay = GetData(ACCEL_YOUT_H);
		az = GetData(ACCEL_ZOUT_H);
    //mpu6050_get_data(&gx, &gy, &gz, &ax, &ay , &az, &temperature);

		
	  acc_filter();

	  gx -= GX_OFFSET;
	  ax -= AX_OFFSET; 
	  ay -=	AY_OFFSET;
	  az -=	AZ_OFFSET;
  
	  angle_dot = gx * GYRO_SCALE;  //+-2000  0.060975 ¡ã/LSB
      angle = atan(ay / sqrt(ax * ax + az * az ));
      angle = angle * 57.295780;    //180/pi

	  kalman_filter(angle, angle_dot, &f_angle, &f_angle_dot);

	  receive_parameter(receive_data);

	  pid(f_angle, f_angle_dot);
		//printf("%d,%d,%d,%d,%d,%d,%d\r",gx,gy,gz,ax,ay,az,temp);


			i = 0;
//			a+=0.1;
//			if(a>3.14)  a=-3.14; 
//			DataScope_Get_Channel_Data(500*sin(a), i++ );
//			DataScope_Get_Channel_Data(500* tan(a), i++ );
//			DataScope_Get_Channel_Data( 500*cos(a), i++ ); 
//			DataScope_Get_Channel_Data( 100*a , i++ );   
			DataScope_Get_Channel_Data(ax, i++ );
			DataScope_Get_Channel_Data(ay , i++ );
			DataScope_Get_Channel_Data(az, i++ );
			DataScope_Get_Channel_Data(gx, i++ ); 
			DataScope_Get_Channel_Data(gy, i++ );  
			DataScope_Get_Channel_Data(gz , i++);
			Send_Count = DataScope_Data_Generate(i);
			for( i = 0 ; i < Send_Count; i++) 
			{
				while((USART1->SR&0X40)==0);  
				USART1->DR = DataScope_OutPut_Buffer[i]; 
			}
			delay_ms(50); //20HZ
		} 
	while (1)             
	{
		if(flag10Hz){//100ms
			flag10Hz = 0;
			LedSet(led6,status);
			if(status)
				status = 0;
			else
				status = 1;
		}
		if(flag50Hz){
			flag50Hz = 0;
		}
		if(flag100Hz){//10ms
			flag100Hz = 0;
		}
	}
}

